#ifndef __CONTROL_DISTANCE_H__
#define __CONTROL_DISTANCE_H__

#include <xqueue.h>
#include <xtask.h>
#include <rrcon.h>

#include "sensors_read.h"
#include "control_distance.h"

namespace control_test {
    using namespace kthrobot;

    class DistanceControler : public XTask {
    public:
        DistanceControler(SensorsDataQueue *data_queue);
        virtual ~DistanceControler();
        void TaskCode();

        void set_target(int sensor, int distance_cm);


    private:
        DistanceControler() : XTask("") {} // Force to specify parameters
        SensorsDataQueue *data_queue_;
        bool target_set_;
        int target_sensor_;
        int target_distance_;

        int get_current_distance();
        void set_both_motors(int speed);
    };
};

#endif // __CONTROL_DISTANCE_H__
